#!/usr/bin/env python3
# coding: utf-8

"""
last edited at 2019-7-21
by Stone at BlueNet
"""

X_TAR, Y_TAR = 0, 0  # tar: target, 小球的目标位置，该位置在机械爪中央。
#                    # 如果抓球时，小车移动，而摄像头位置和机械爪位置固定，机械爪总是位于图像中的某一位置，是一常量，
#                    # 则使用该参数。
#                    # 如果抓球时，小车不动，而通过机械爪的运动抓球，机械爪在图像中的位置是变化的，
#                    # 则实时识别机械爪的位置，并作为参数传递给PosController，不使用此处参数。

X_CTR = int(640/2)        # ctr: center, 图像中轴竖线的坐标
TAN_TH_LOOKDOWN = 100/20  # tan threshold lookdown,当小球在该角度之内时，停止旋转运动，开始前进运动


class BallPosController(object):
    """
    位置反馈控制器，控制目标是使小球的位置趋于图像中轴线和目标位置上。接受recognizer识别出的
    小球坐标为输入，计算并输出相应的运动指令，传递给下一级程序。

    目前的控制策略使，先通过小车的旋转运动使小球位于图像轴线上，然后再通过平移运动使小球位于
    图像的目标位置（机械爪中央）。
    """

    def __init__(self, state, pos_tar=(X_TAR, Y_TAR)):
        self._x_tar = pos_tar[0]
        self._y_tar = pos_tar[1]  # 该值可以为负值
        self._state = state  # State对象，用于获取小球及机械爪的位置。

        self._Kp = 0  # pid k, proposal
        self._Ki = 0  # pid k, integral

    def get_output_lookdown(self, pos_tar=None):
        if pos_tar:
            self._x_tar, self._y_tar = pos_tar

        xb, yb = self._state.get_pos_ball()  # b:ball, x_ball, y_ball
        error_x = xb - self._x_tar
        error_y = yb - self._y_tar




